#include "JasonActionMove.h"

CJasonActionMove::CJasonActionMove( ros::NodeHandle &rNode, const jason_msgs::action::ConstPtr& poActionMsg )
: CJasonAction()
, m_rNode( rNode )
, m_wwId( poActionMsg->action_id )
, m_strAction( poActionMsg->action )
, m_strAgent( poActionMsg->agent )
, m_bnHasInitialOdometry( false )
, m_bnBlocked( false )
{
    const std::string strDirection( boost::to_upper_copy( poActionMsg->parameters[0] ) );

    if ( strDirection == "FORWARD" ) {

        m_oMsg.linear.x = 0.5;
    }
    else if ( strDirection == "BACKWARD" ) {

        m_oMsg.linear.x = -0.5;
    }
    else {

        m_oMsg.linear.x = 0.0;          
    }

    m_fDistance     = boost::lexical_cast< float >( poActionMsg->parameters[1] );
    m_fLastDistance = 0.0f;
}

const float CJasonActionMove::GetDistance( const geometry_msgs::Point &rCurrPoint ) {
    
    return (const float)sqrt( 
        ( m_oInitialPosition.x - rCurrPoint.x ) * ( m_oInitialPosition.x - rCurrPoint.x ) +
        ( m_oInitialPosition.y - rCurrPoint.y ) * ( m_oInitialPosition.y - rCurrPoint.y ) +
        ( m_oInitialPosition.z - rCurrPoint.z ) * ( m_oInitialPosition.z - rCurrPoint.z ) );
}

void CJasonActionMove::Start() {
    
    if ( !m_bnDone ) {
        
        m_oCmdvelPub = m_rNode.advertise< geometry_msgs::Twist     >( "/fault_injector/cmd_vel", 10 );
        m_oEventPub  = m_rNode.advertise< jason_msgs::event        >( "/jason/event", 10 );
        m_oOdomSub   = m_rNode.subscribe< nav_msgs::Odometry       >( "/fault_injector/odom"       , 10, &CJasonActionMove::ReceiveOdometry, this );
        m_oBumperSub = m_rNode.subscribe< kobuki_msgs::BumperEvent >("/mobile_base/events/bumper",1, &CJasonActionMove::BumperStatus, this );
        
        m_oCAclient = m_rNode.serviceClient< ft_nodes::collision_check >( "collision_avoidance" );
        
        m_oProgressTimeout = boost::posix_time::microsec_clock::local_time();
    }
}

void CJasonActionMove::Step() {
    
    if ( m_bnHasInitialOdometry && !m_bnDone ) {
    
        ft_nodes::collision_check check;
        check.request.twist.twist = m_oMsg;
        if ( m_oCAclient.call( check ) ) {
        
            if ( check.response.ack ) {
            
                m_oCmdvelPub.publish( m_oMsg );
                m_bnBlocked = false;
            }
            else {
                
                ROS_INFO("stuck!");
                m_bnBlocked = true;
            }
        }
    }
}

void CJasonActionMove::BumperStatus( const kobuki_msgs::BumperEvent::ConstPtr& msg ) {
    
    if ( msg.get()->state ) {
    
        m_bnDone = true;

        jason_msgs::event e;
        e.agent = m_strAgent;
        e.event_type = "ActionFeedback";
        e.parameters.push_back( m_strAction );
        e.parameters.push_back( boost::str( boost::format( "%d" ) % m_wwId ) );
        e.parameters.push_back( "blocked" );
        e.parameters.push_back( boost::str( boost::format( "%f" ) % 0 ) );

        m_oEventPub.publish( e );

        m_oOdomSub.shutdown();
        m_oBumperSub.shutdown();
    }
}
    
void CJasonActionMove::ReceiveOdometry( const nav_msgs::Odometry::ConstPtr& msg ) {

    if ( !m_bnHasInitialOdometry ) {
        
        m_oInitialPosition = msg.get()->pose.pose.position;
        tf::quaternionMsgToTF( msg.get()->pose.pose.orientation, m_oInitialPose );
        
        m_bnHasInitialOdometry = true;
    }
    else {
        

        tf::Quaternion oCurrPose;
        tf::quaternionMsgToTF( msg.get()->pose.pose.orientation, oCurrPose );
        if ( fabs( m_oInitialPose.dot( oCurrPose ) ) < 0.99 ) {
            
                m_bnDone = true;

                jason_msgs::event e;
                e.agent = m_strAgent;
                e.event_type = "ActionFeedback";
                e.parameters.push_back( m_strAction );
                e.parameters.push_back( boost::str( boost::format( "%d" ) % m_wwId ) );
                e.parameters.push_back( "bad_move" );

                m_oEventPub.publish( e );

                m_oOdomSub.shutdown();
                m_oBumperSub.shutdown();
                return;
        }
        
        const float fCurrDistance = GetDistance( msg.get()->pose.pose.position );
        ROS_INFO("distance %f", fCurrDistance );
        
        if ( fCurrDistance >= m_fDistance ) {
            
            m_bnDone = true;

            jason_msgs::event e;
            e.agent = m_strAgent;
            e.event_type = "ActionFeedback";
            e.parameters.push_back( m_strAction );
            e.parameters.push_back( boost::str( boost::format( "%d" ) % m_wwId ) );
            e.parameters.push_back( "Ok" );

            m_oEventPub.publish( e );

            m_oOdomSub.shutdown();
            m_oBumperSub.shutdown();
            
        }
        else if ( fCurrDistance == m_fLastDistance ) {
            
            boost::posix_time::time_duration diff = boost::posix_time::microsec_clock::local_time() - m_oProgressTimeout;
            const int nElapsedtime = diff.total_milliseconds();

            if ( m_bnBlocked ) {
                
                if ( nElapsedtime > 5000 ) {
                
                    m_bnDone = true;

                    jason_msgs::event e;
                    e.agent = m_strAgent;
                    e.event_type = "ActionFeedback";
                    e.parameters.push_back( m_strAction );
                    e.parameters.push_back( boost::str( boost::format( "%d" ) % m_wwId ) );
                    e.parameters.push_back( "blocked" );
                    e.parameters.push_back( boost::str( boost::format( "%f" ) % fCurrDistance ) );

                    m_oEventPub.publish( e );

                    m_oOdomSub.shutdown();
                    m_oBumperSub.shutdown();
                }
                else {
                    
                    ROS_INFO("stuck time %d ms", nElapsedtime );
                }
            }
            else if ( nElapsedtime > 1000 ) {
                
                m_bnDone = true;

                jason_msgs::event e;
                e.agent = m_strAgent;
                e.event_type = "ActionFeedback";
                e.parameters.push_back( m_strAction );
                e.parameters.push_back( boost::str( boost::format( "%d" ) % m_wwId ) );
                e.parameters.push_back( "no_progress" );
                e.parameters.push_back( boost::str( boost::format( "%f" ) % fCurrDistance ) );

                m_oEventPub.publish( e );

                m_oOdomSub.shutdown();
                m_oBumperSub.shutdown();
            }
        }
        else {
            
            if ( m_fDistance - fCurrDistance < 1.0f ) {
            
                m_oMsg.linear.x = ( m_oMsg.linear.x < 0.0f ? -0.2f : 0.2f );
            }
            m_oProgressTimeout = boost::posix_time::microsec_clock::local_time();
        }
        m_fLastDistance = fCurrDistance;
    }
}

CJasonActionMove::~CJasonActionMove() {
    
}
